#pragma once
#include "CAction.h"

#define VELOCITY_CONTROL_ID 16
#define MAX_FORCE 0.03
#define DEFAULT_KP_VEL 0.03
#define DEFAULT_KD_VEL 0.00
#define DEFAULT_KI_VEL 0.12
#define uint unsigned int 
using namespace std;

class CVelCrtl :
	public CAction
{
public:
	CVelCrtl(void);
	CVelCrtl( CSeaAmp &seaAmp ) { Init( seaAmp ); }
	~CVelCrtl(void);


	bool Init( CSeaAmp &seaAmp ) { return CAction::Init( seaAmp, VELOCITY_CONTROL_ID ); }

	void DoAct( void );

	void SetKP( double p ) { Kp = p; }
	double GetKP( void ) { return Kp; }

	void SetKI( double i ) { Ki = i; }
	double GetKI( void ) { return Ki; }

	void SetKD( double d ) { Kd = d; }
	double GetKD( void ) { return Kd; }
	
private:
	double Kp;
	double Ki;
	double Kd;

	double delta[3];

	double output;

	ofstream VelocityData;

	//CQueryTimer timer;
	double timeInterval;

	double a;
};

